#include "PID.h"
#include <math.h>
PID speedcircle;
int Increase = 0; // 增量输出
int PID_Increase(PID* sptr, float* PID, int NowPoint, int SetPoint)
{
    // 当前误差，定义为寄存器变量，只能用于整型和字符型变量，提高运算速度
    int iError = 0; // 当前误差

    iError = SetPoint - NowPoint; // 计算当前误差

    float out_i = PID[KI] * iError;
    if (fabsf(sptr->LastError) > 1000. && fabsf(out_i) > 1000.)
        out_i = 0.;

    Increase += PID[KP] * (iError - sptr->LastError) +
        out_i +
        PID[KD] * (iError - 2.0f * sptr->LastError + sptr->PrevError);

    sptr->PrevError = sptr->LastError; // 更新前次误差
    sptr->LastError = iError;          // 更新上次误差
    sptr->LastData = NowPoint;         // 更新上次数据
    if (PID[KT] < Increase && PID[KT] != 0)
        Increase = (int)(PID[KT]);
    else if (-PID[KT] > Increase && PID[KT] != 0)
        Increase = (int)(-PID[KT]);

    return Increase; // 返回增量
}
// ********************位置式动态PID控制************************************
/*
函数：int PlacePID_Control(PID *sprt, float *PID, int NowPiont, int SetPoint)
功能：位置式动态PID控制
参数：
PID *sprt：      结构体指针
float *PID：     PID数组  （通过数组定义PID值）
int NowPiont： 当前值  （可使用结构体定义变量）
int SetPoint： 设定目标值   转向控制中设定值为0。

说明：  该函数参考其他程序。动态控制一般用于转向控制
返回值： int Realize
eg：Radius = PlacePID_Control(&Turn_PID, Turn[Fres], Difference, 0);// 动态PID控制转向
*/
// 位置式动态PID控制
int PlacePID_Control(PID* sprt, float* PID, int NowPiont, float SetPoint)
{
    // 定义为寄存器变量，只能用于整型和字符型变量，提高运算速度
    int iError,                 // 当前误差
        Actual;                   // 最后得出的实际输出值
    float Kp;                     // 动态P
    iError = SetPoint - NowPiont; // 计算当前误差
    sprt->SumError += iError * 0.01;
    if (sprt->SumError >= PID[KT])
    {
        sprt->SumError = PID[KT];
    }
    else if (sprt->SumError <= -PID[KT])
    {
        sprt->SumError = -PID[KT];
    }
    Kp = (iError * iError / 12500.f) * PID[KP] + PID[KI];
    // Kp = 1.0 * (iError * iError) / PID[KP] + PID[KI]; // P值与差值成二次函数关系，此处P和I不是PID参数，而是动态PID参数，要注意！！！

    Actual = Kp * iError + PID[KD] * ((0.8 * iError + 0.2 * sprt->LastError) - sprt->LastError); // 只用PD
    sprt->LastError = iError;                                                                    // 更新上次误差

    // Actual += sprt->SumError*0.1;
    // Actual = limit(Actual, 300); //限幅

    return Actual;
}
int PID_Realize(PID* sptr, float* PID, int NowPoint, int SetPoint)
{
    // 当前误差，定义为寄存器变量，只能用于整型和字符型变量，提高运算速度
    int iError;  // 当前误差
    float Realize; // 最后得出的实际增量

    iError = SetPoint - NowPoint;       // 计算当前误差      设定减当前
    sptr->SumError += PID[KI] * iError; // 误差积分
    // sptr->SumError = limit(sptr->SumError, PID[KT]); // 积分限幅

    Realize = PID[KP] * iError +
        sptr->SumError +
        PID[KD] * (iError - sptr->LastError); // P  I   D  相加s
    sptr->PrevError = sptr->LastError;              // 更新前次误差
    sptr->LastError = iError;                       // 更新上次误差
    sptr->LastData = NowPoint;                      // 更新上次数据    没用 */

    return Realize; // 返回实际值
}
